#include "board.h"
#include "MotorControl.h"


#define SPD_UPDDATE_PERIOD  10U  // milli sec


float g_spd_m1 = 0;
uint32_t g_pwm_duty = 100;
//SoftTimer_TypeDef tim_spd;


/**@brief Called by timer to periodically calculate speed. */
//static void SpeedRoutine(void) {
//    g_spd_m1 = Spd_CalcRevsPerSecD3(M1.pstEnc, SPD_UPDDATE_PERIOD);
//    g_spd_m1 /= 1000 * 20U;
//}


void demo_motor(void) {
    
    Mot_InitSys();
//    SfTim_InitSys();
//    SfTim_Init(&tim_spd, SPD_UPDDATE_PERIOD*100U, SpeedRoutine);
    
    
    M1.pstHBdg->ParamChanged = true;
    M1.pstHBdg->Param.State = HB_State_Run;
    M1.pstHBdg->Param.DirMode = HB_DirMode_ALBHasForward;
    
    M1.pstEnc->ParamChanged = true;
    M1.pstEnc->Param.DirMode = Enc_DirMode_1;
    M1.pstEnc->Param.PulsPerRev = 26 - 1;
    while (1) {
        Mot_Update(&M1);
        
        HBridge_SetPwmDuty(M1.pstHBdg, g_pwm_duty);
        g_spd_m1 = M1.ParamRO.s32d3SpeedFdbk / (1000.0 * 20);
    }
}

